#include <iostream>
#include <string>
#include <deque>
#include <thread>
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/NavSatFix.h>
#include <std_msgs/String.h>
#include <yaml-cpp/yaml.h>


class bag2bagProcess
{
public:
    std::string root_dir = ROOT_DIR;
    std::string newBagFile_dir;
    std::string outBagFiles;
    // const std::string outBagFiles = std::string(std::string(ROOT_DIR)) + "data/grassland_01.bag";
    // int reflash = system((std::string("exec rm ") + outBagFiles).c_str());

    ros::NodeHandle nh;
    ros::Subscriber subLegOdom;
    ros::Subscriber subLidarOdom;
    ros::Subscriber subLidar;
    ros::Subscriber subImu;
    ros::Subscriber subJointState;
    ros::Subscriber subNavsatfix;

    rosbag::Bag newBag;
    bool flagRecord = false;

    std::string leg_odom_Topic = "/leg_odom";
    std::string lidar_odom_Topic = "/Odometry";
    std::string lidar_Topic = "/velodyne_points";
    std::string imu_Topic = "/xsens/imu/data";
    std::string jointState_Topic = "/joint_states";
    std::string navsatfix_Topic ="/navsatfix";

    std::mutex odomLegLock;
    std::mutex odomLidarLock;



    bag2bagProcess()
    {
        YAML::Node yaml;
        yaml = YAML::LoadFile(std::string(std::string(ROOT_DIR)) + "config/bag2bagProcessConfig.yaml");
        newBagFile_dir = yaml["newBagFile_dir"].as<std::string>();
        outBagFiles = std::string(std::string(ROOT_DIR)) + newBagFile_dir;
        int reflash = system((std::string("exec rm ") + outBagFiles).c_str());

        subLegOdom    = nh.subscribe<nav_msgs::Odometry>(leg_odom_Topic, 1000, &bag2bagProcess::legOdometry_handle, this, ros::TransportHints().tcpNoDelay());
        subLidarOdom  = nh.subscribe<nav_msgs::Odometry>(lidar_odom_Topic, 1000, &bag2bagProcess::lidarOdometry_handle, this, ros::TransportHints().tcpNoDelay());
        subLidar      = nh.subscribe<sensor_msgs::PointCloud2>(lidar_Topic, 1000, &bag2bagProcess::lidar_handle, this, ros::TransportHints().tcpNoDelay());
        subImu        = nh.subscribe<sensor_msgs::Imu>(imu_Topic, 1000, &bag2bagProcess::imu_handle, this, ros::TransportHints().tcpNoDelay());
        subJointState = nh.subscribe<sensor_msgs::JointState>(jointState_Topic, 1000, &bag2bagProcess::jointState_handle, this,  ros::TransportHints().tcpNoDelay());
        subNavsatfix  = nh.subscribe<sensor_msgs::NavSatFix>(navsatfix_Topic, 1000, &bag2bagProcess::navsatfix_handle, this, ros::TransportHints().tcpNoDelay());

        newBag.open(outBagFiles, rosbag::bagmode::Write);
    }

    
    /**************************************************************
     * @brief 订阅legodom的回调函数
     * 重新赋值时间戳
     * 
     * @param msgIn 
     *************************************************************/
    void legOdometry_handle(const nav_msgs::Odometry::ConstPtr& msgIn)
    {
        std::lock_guard<std::mutex> lock1(odomLegLock);
        nav_msgs::Odometry data = *msgIn;
        data.header.stamp = ros::Time::now();

        if (data.pose.pose.position.z > 0.2)
            flagRecord = true;
        
        if (flagRecord)
        {
            newBag.write(leg_odom_Topic, ros::Time::now(), data);
            // ROS_INFO("\033[1;32m lidar_end_time: %f; \033[0m", data.header.stamp.toSec());
        }
        
    }

    /**************************************************************
     * @brief 订阅lidarodom的回调函数
     * 重新赋值时间戳
     * 
     * @param msgIn 
     *************************************************************/
    void lidarOdometry_handle(const nav_msgs::Odometry::ConstPtr& msgIn)
    {
        std::lock_guard<std::mutex> lock2(odomLidarLock);
        nav_msgs::Odometry data = *msgIn;
        data.header.stamp = ros::Time::now();

        if (flagRecord)
        {
            newBag.write(lidar_odom_Topic, ros::Time::now(), data);
        }
    }

    /**************************************************************
     * @brief 订阅lidar的回调函数
     * 重新赋值时间戳
     * 
     * @param msgIn 
     *************************************************************/
    void lidar_handle(const sensor_msgs::PointCloud2::ConstPtr& msgIn)
    {
        sensor_msgs::PointCloud2 data = *msgIn;
        data.header.stamp = ros::Time::now();

        if (flagRecord)
        {
            newBag.write(lidar_Topic, ros::Time::now(), data);
        }
    }

    /**************************************************************
     * @brief 订阅imu的回调函数
     * 重新赋值时间戳
     * 
     * @param msgIn 
     *************************************************************/
    void imu_handle(const sensor_msgs::Imu::ConstPtr& msgIn)
    {
        sensor_msgs::Imu data = *msgIn;
        data.header.stamp = ros::Time::now();

        if (flagRecord)
        {
            newBag.write(imu_Topic, ros::Time::now(), data);
        }
    }


    /***************************************************************
     * @brief 订阅关节角度的回调函数
     * 重新赋值时间戳
     * 
     * @param msgIn 
     **************************************************************/
    void jointState_handle(const sensor_msgs::JointState::ConstPtr& msgIn)
    {
        sensor_msgs::JointState data = *msgIn;
        data.header.stamp = ros::Time::now();

        if (flagRecord)
        {
            newBag.write(jointState_Topic, ros::Time::now(), data);
        }
    }


    /***************************************************************
     * @brief 订阅GPS的回调函数
     * 重新赋值时间戳
     * 
     * @param msgIn 
     **************************************************************/
    void navsatfix_handle(const sensor_msgs::NavSatFix::ConstPtr& msgIn)
    {
        sensor_msgs::NavSatFix data = *msgIn;
        data.header.stamp = ros::Time::now();

        if (flagRecord)
        {
            newBag.write(navsatfix_Topic, ros::Time::now(), data);
        }
    }




    void processTopicsInfo()
    {
        ros::Rate rate(5000);
        while (ros::ok())
        {
            reviseTopicsInfo();

            rate.sleep();
        }
    }

    void reviseTopicsInfo()
    {

    }



};







int main(int argc, char** argv)
{ 
    ros::init(argc, argv, "bag2bagProcess");

    bag2bagProcess MO;

    ROS_INFO("\033[1;32m----> bag2bag Procee Started.\033[0m");

    std::thread bag2bagProcessThread(&bag2bagProcess::processTopicsInfo, &MO);

    ros::spin();

    bag2bagProcessThread.join();

    return 0;
}